/*
* @File Name freecars.c
* @File Path M:\MAS2\PRM_Robotic_Arm\PRMCcode\coding\freecars\freecars.c
* @Author: Ruige_Lee
* @Date:   2019-09-01 17:27:04
* @Last Modified by:   Ruige_Lee
* @Last Modified time: 2019-09-26 19:25:22
* @Email: 295054118@whut.edu.cn
* @page: https://whutddk.github.io/
*/


#include <stdint.h>
#include "xil_printf.h"
#include "xuartps.h"

#include "freecars.h"
#include "task/include.h"


XUartPs uart1_instance_ptr;

extern void task_pickAndPlace();
extern void task_router();
extern void task_collide();

int32_t uart_hw_setup()
{
	XUartPs_Config *uart_config;

	
	uart_config = XUartPs_LookupConfig(XPAR_XUARTPS_1_DEVICE_ID);
	
	if (XST_SUCCESS != XUartPs_CfgInitialize(&uart1_instance_ptr, uart_config, uart_config -> BaseAddress))
	{
		return XST_FAILURE;
	}
	
	if ( XST_SUCCESS != XUartPs_SelfTest(&uart1_instance_ptr) )
	{
		return XST_FAILURE;
	}
	
	XUartPs_ResetHw( (u32)(&uart1_instance_ptr) );
	

	
	return 0;
}

void Uart_Putchar(uint8_t data)
{
	XUartPs_Config *uart_config;


	uart_config = XUartPs_LookupConfig(XPAR_XUARTPS_1_DEVICE_ID);

	XUartPs_SendByte(uart_config -> BaseAddress, data);
}

uint8_t UART_GetChar()
{
	XUartPs_Config *uart_config;

	uart_config = XUartPs_LookupConfig(XPAR_XUARTPS_1_DEVICE_ID);

	return XUartPs_RecvByte(uart_config -> BaseAddress);
}

void Uart_printf(uint8_t * str)
{
	while( *(str) != 0 )
	{
		Uart_Putchar(*(str));
	}
}

void task_rec()
{
	if ( XUartPs_IsReceiveData(XPAR_XUARTPS_1_BASEADDR) )
	{
		freecars_isr();
		// Uart_Putchar(UART_GetChar());
	}
}




uint8_t uSendBuf[ UartDataNum*2 ] = { 0 };
uint8_t FreeCarsDataNum=UartDataNum * 2;

double UartData[9] = { 0 };
SerialPortType SerialPortRx;


void push(uint8_t chanel,uint16_t data)
{
	uSendBuf[ chanel*2 ] = data / 256;
	uSendBuf[ chanel*2 + 1 ] = data % 256;
}

void sendDataToScope()
{
	uint8_t i,sum = 0; 
	
	Uart_Putchar(251);
	Uart_Putchar(109);
	Uart_Putchar(37);

	sum += (251);     
	sum += (109);
	sum += (37);
	for( i = 0; i < FreeCarsDataNum; i++ )
	{
		Uart_Putchar(uSendBuf[i] );
		sum += uSendBuf[i];      
	}
	Uart_Putchar( sum );
}

void freecars_isr(void)
{
	int32_t i,b,d1;
	uint32_t d;
  
	{
	SerialPortRx.Data = UART_GetChar(); 
   
		if( SerialPortRx.Stack < UartRxBufferLen )
		{
			SerialPortRx.Buffer[ SerialPortRx.Stack ++ ] = SerialPortRx.Data;
	  
			if(   SerialPortRx.Stack >= UartRxDataLen 
		 		&& SerialPortRx.Buffer[SerialPortRx.Stack - UartRxDataLen] == 0xff
				&& SerialPortRx.Buffer[SerialPortRx.Stack - UartRxDataLen+1] == 0x55
				&& SerialPortRx.Buffer[SerialPortRx.Stack - UartRxDataLen+2] == 0xaa
				&& SerialPortRx.Buffer[SerialPortRx.Stack - UartRxDataLen+3] == 0x10 )
			{  
				SerialPortRx.Check = 0;
				b = SerialPortRx.Stack - UartRxDataLen; 
				for( i = b; i < SerialPortRx.Stack - 1; i++ ) 
				{
					SerialPortRx.Check += SerialPortRx.Buffer[i];
				}
		
				if( SerialPortRx.Check == SerialPortRx.Buffer[SerialPortRx.Stack-1] )
				{
					for( i = 0; i < 9; i ++ )
					{
						d = SerialPortRx.Buffer[ b + i*4 + 4 ]*0x1000000L + SerialPortRx.Buffer[ b + i*4 + 5 ]*0x10000L + SerialPortRx.Buffer[ b + i*4 + 6 ]*0x100L + SerialPortRx.Buffer[ b + i*4 + 7 ];
						if( d > 0x7FFFFFFF )
						{
							d1 = 0x7FFFFFFF - d;
						}
						else
						{
							d1 = d;
						}
						UartData[i] = d1;
						UartData[i] /= 65536.0;
					}
					UartDebug(); 
				}
				SerialPortRx.Stack = 0;
			}
			else if(   SerialPortRx.Stack >= UartRxCmdLen 
						&& SerialPortRx.Buffer[SerialPortRx.Stack - UartRxCmdLen] == 0xff
						&& SerialPortRx.Buffer[SerialPortRx.Stack - UartRxCmdLen+1] == 0x55
						&& SerialPortRx.Buffer[SerialPortRx.Stack - UartRxCmdLen+2] == 0xaa
						&& SerialPortRx.Buffer[SerialPortRx.Stack - UartRxCmdLen+3] == 0x77 )//cmd
			{
				SerialPortRx.Check = 0;
				b = SerialPortRx.Stack - UartRxCmdLen;
				for( i = b; i < SerialPortRx.Stack - 1; i++ ) 
				{
					SerialPortRx.Check += SerialPortRx.Buffer[i];
				}
				if( SerialPortRx.Check == SerialPortRx.Buffer[SerialPortRx.Stack-1] )
				{
					UartCmd(UartCmdNum,UartCmdData);
				}
				SerialPortRx.Stack = 0;
			}
		}
		else
		{
			SerialPortRx.Stack = 0;
		} 
	}
}


void UartDebug()
{
	// (int16_t)UartData[0];
}


extern uint8_t global_release;
void UartCmd(uint8_t cmdnum,uint8_t cmddata)
{


	switch(cmddata)
	{
		case(6):break;//F6
		case(7):break;//F7
		case(8):break;//F8
		case(9):break;//F9
		case(10):
			global_release = 1;
			//task_router();
			// task_pickAndPlace();
		break;//F10
		case(11):stepper_PowerUp(0);stepper_PowerUp(1);break;//F11
		case(12):stepper_PowerDown(0);stepper_PowerDown(1);break;//F12
		case(100):break;//PAUSE
		case(101):stepper_enable(0);stepper_enable(1);
		break;//HOME
		case(102):break;//PG UP
		case(103):break;//PF DN
		case(104):stepper_disable(0);stepper_disable(1);break;//END
	}

}

